﻿/*******************************************************************
 *     JameE
 *******************************************************************
  Copyright 2010 Clifford Champion and JameE contributors.
  
  This file is part of JameE.
  
  JameE is free software: you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation, either version 3 of the License, or
  (at your option) any later version.
  
  JameE is distributed in the hope that it will be useful,
  but WITHOUT ANY WARRANTY; without even the implied warranty of
  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  GNU General Public License for more details.
  
  You should have received a copy of the GNU General Public License
  along with JameE.  If not, see <http://www.gnu.org/licenses/>.
 ******************************************************************/
 
/// <reference path="../Base/Classer.js" />
/// <reference path="../Base/IncludeLoader.js" />

$class(
"JameE.Engine.PathFinding$SearchNode",
{
	iVertexId: 0,
	oLocation: null,
	fCostFromStart: null,
	fCostToEndEstimate: Number.POSITIVE_INFINITY,
	oPreviousNode: null,

	calculateCostFromStart: function () {
		var fCostFromStart = 0.0;
		var oPrev = this.oPreviousNode;
		if (oPrev !== null) {
			fCostFromStart = oPrev.fCostFromStart;
			fCostFromStart += this.oLocation.subtract(oPrev.oLocation).magnitudeSquared();
		}
		this.fCostFromStart = fCostFromStart;
	},

	calculateCostToEndEstimate: function (oEndPosition) {
		$require_type(oEndPosition, "Math.Vector3");

		this.fCostToEndEstimate = this.oLocation.subtract(oEndPosition).magnitudeSquared() - 1.0;
	}
});

$class(
"JameE.Engine.PathFinding",
{
	/////////////////////
	// CONSTANTS
	/////////////


	/////////////////////
	// FIELDS
	/////////////
	_oGraph: null,
	_arr_oOpenNodes: null,
	_arr_oClosedNodes: null,


	/////////////////////
	// CONSTRUCTORS
	/////////////
	_ctor: function () {
		this._oGraph = $argument("graph", "Math.Graph") || null;
		this._arr_oClosedNodes = [];
		this._arr_oOpenNodes = [];
	},

	locatePathFromTo: function (oFrom, oTo) {
		$require_type(oFrom, "Math.Vector3");
		$require_type(oTo, "Math.Vector3");

		var oGraph = this._oGraph;

		var iStartVertexId = oGraph.findNearestVertex(oFrom);
		if (iStartVertexId === null) {
			return null;
		}
		var iEndVertexId = oGraph.findNearestVertex(oTo);
		if (iEndVertexId === null) {
			return null;
		}

		if (iStartVertexId === iEndVertexId) {
			return [iEndVertexId];
		}

		var oStartVertexPosition = oGraph.getVertex(iStartVertexId);
		var oEndVertexPosition = oGraph.getVertex(iEndVertexId);

		var arr_oOpenNodes = this._arr_oOpenNodes;
		var arr_oClosedNodes = this._arr_oClosedNodes;

		arr_oOpenNodes.length = 0;
		arr_oClosedNodes.length = 0;

		// add start node
		var oNode_Start = new JameE.Engine.PathFinding$SearchNode();
		oNode_Start.iVertexId = iStartVertexId;
		oNode_Start.oLocation = oStartVertexPosition;
		oNode_Start.fCostFromStart = 0.0;
		oNode_Start.calculateCostToEndEstimate(oEndVertexPosition);
		oNode_Start.oPreviousNode = null;

		arr_oOpenNodes.push(oNode_Start);

		while (true) {
			if (arr_oOpenNodes.length === 0) {
				return null;
			}

			// grab latest node from the open list
			var oNode = arr_oOpenNodes.pop();
			if (oNode.iVertexId === iEndVertexId) {
				return this._returnVertexIdsPath(oNode);
			}

			// add node to closed set. resort.
			arr_oClosedNodes.push(oNode);
			arr_oClosedNodes.sort(
				function (oNodeA, oNodeB) {
					return oNodeA.iVertexId - oNodeB.iVertexId;
				}
			);

			// inspect each neighboring vertex
			oGraph.forEachEdgeOfVertex(
				oNode.iVertexId,
				function (iEdgeId, iVertexIdA, oVertexA, iVertexIdB, oVertexB) {

					// force 'A' to be current vertex.
					if (iVertexIdA !== oNode.iVertexId) {
						var tmp = iVertexIdA;
						iVertexIdA = iVertexIdB;
						iVertexIdB = tmp;
						tmp = oVertexA;
						oVertexA = oVertexB;
						oVertexB = tmp;
					}

					// is vertex B already on the closed list
					if (JameE
						.Utils
						.Array
						.binarySearch(
							arr_oClosedNodes,
							function (oElement) { return iVertexIdB - oElement.iVertexId; }
						) !== -1
					) {
						return true; // continue
					}

					// is vertex B already on the open list?
					var iIndexOfVertexBInOpenList;
					iIndexOfVertexBInOpenList
						= JameE
						.Utils
						.Array
						.sequentialSearch(
							arr_oOpenNodes,
							function (oElement) { return iVertexIdB - oElement.iVertexId; }
						)
					;

					// estimate new cost
					var fCostFromStartToNode = oNode.fCostFromStart;
					var fCostFromNodeToB = oVertexB.subtract(oNode.oLocation).magnitudeSquared();
					var fCostFromBToEnd = oEndVertexPosition.subtract(oVertexB).magnitudeSquared();
					var fApproximateTotalCost = fCostFromStartToNode + fCostFromNodeToB + fCostFromBToEnd;

					var oNode_B;
					if (iIndexOfVertexBInOpenList !== -1) {
						oNode_B = arr_oOpenNodes[iIndexOfVertexBInOpenList];
						if (fApproximateTotalCost >= oNode_B.fCostFromStart + oNode_B.fCostToEndestimate) {
							return true; // continue;
						}
					}
					else {
						oNode_B = new JameE.Engine.PathFinding$SearchNode();
						oNode_B.iVertexId = iVertexIdB;
						oNode_B.oLocation = oVertexB;
						arr_oOpenNodes.push(oNode_B);
					}

					oNode_B.oPreviousNode = oNode;
					oNode_B.calculateCostFromStart();
					oNode_B.fCostToEndEstimate = fCostFromBToEnd;
				}
			);

			// put most promising nodes at the top of the stack
			arr_oOpenNodes.sort(
				function (oNodeA, oNodeB) {
					return oNodeA.fCostToEndEstimate - oNodeB.fCostToEndEstimate;
				}
			);
		}
	},

	_returnVertexIdsPath: function (oNodeEnding) {
		var arr_iVertexIds = [];
		var oNodeTemp = oNodeEnding;
		do {
			arr_iVertexIds.push(oNodeTemp.iVertexId);
			oNodeTemp = oNodeTemp.oPreviousNode;
		} while (oNodeTemp !== null);
		arr_iVertexIds.reverse();
		return arr_iVertexIds;
	}
});

$include_notify("Engine/PathFinding.js");
